Relative State Estimation perform mutually localization between two mobile agents undergoing six-degree-of-freedom motion. Based on the principle of circular motion, the estimation accuracy is sensitive to nonlinear rotations of the reference platform, particularly under large inter-platform distances. This phenomenon is even obvious for linearized kinematics, because cumulative linearization errors significantly degrade precision. In virtual reality (VR) applications, this manifests as substantial positional errors in 6-DoF controller tracking during rapid rotations of the head-mounted display. The linearization errors introduce drift in the estimate and render the estimator inconsistent. In the field of odometry, IMU preintegration is proposed as a kinematic observation to enable efficient relinearization, thus mitigate linearized error. Building on this theory, we propose dual preintegration, a novel observation integrating IMU preintegration from both platforms. This method serves as kinematic constraints for consecutive relative state and supports efficient relinearization. We also perform observability analysis of the state and analytically formulate the accordingly null space. Algorithm evaluation encompasses both simulations and real-world experiments. Multiple nonlinear rotations on the reference platform are simulated to compare the precision of the proposed method with that of other state-of-the-art (SOTA) algorithms. The field test compares the proposed method and SOTA algorithms in the application of VR controller tracking from the perspectives of bias observability, nonlinear rotation, and background texture. The results demonstrate that the proposed method is more precise and robust than the SOTA algorithms.

The inconsistency issue in the Visual-Inertial Navigation System (VINS) is a long-standing and fundamental challenge. While existing studies primarily attribute the inconsistency to observability mismatch, these analyses are often based on simplified theoretical formulations that consider only prediction and SLAM correction. Such formulations fail to cover the non-standard estimation steps, such as MSCKF correction and delayed initialization, which are critical for practical VINS estimators. Furthermore, the lack of a comprehensive understanding of how inconsistency dynamically emerges across estimation steps has hindered the development of precise and efficient solutions. As a result, current approaches often face a trade-off between estimator accuracy, consistency, and implementation complexity. To address these limitations, this paper proposes a novel analysis framework termed Unobservable Subspace Evolution (USE), which systematically characterizes how the unobservable subspace evolves throughout the entire estimation pipeline by explicitly tracking changes in its evaluation points. This perspective sheds new light on how individual estimation steps contribute to inconsistency. Our analysis reveals that observability misalignment induced by certain steps is the antecedent of observability mismatch. Guided by this insight, we propose a simple yet effective solution paradigm, Unobservable Subspace Alignment (USA), which eliminates inconsistency by selectively intervening only in those estimation steps that induce misalignment. We design two USA methods: transformation-based and re-evaluation-based, both offering accurate and computationally lightweight solutions. Extensive simulations and real-world experiments validate the effectiveness of the proposed methods.

This paper presents a novel approach to address the inconsistency problem caused by observability mismatch in visual-inertial navigation systems (VINS). The key idea involves applying a linear time-varying transformation to the error-state within the Error-State Kalman Filter (ESKF). This transformation ensures that \textrr{the unobservable subspace of the transformed error-state system} becomes independent of the state, thereby preserving the correct observability of the transformed system against variations in linearization points. We introduce the Transformed ESKF (T-ESKF), a consistent VINS estimator that performs state estimation using the transformed error-state system. Furthermore, we develop an efficient propagation technique to accelerate the covariance propagation based on the transformation relationship between the transition and accumulated matrices of T-ESKF and ESKF. We validate the proposed method through extensive simulations and experiments, demonstrating better (or competitive at least) performance compared to state-of-the-art methods. The code is available at github.com/HITCSC/T-ESKF.

In this paper, we investigate the inconsistency problem arising from observability mismatch that frequently occurs in nonlinear systems such as multi-robot cooperative localization and simultaneous localization and mapping. For a general nonlinear system, we discover and theoretically prove that the unobservable subspace of the EKF estimator system is independent of the state and belongs to the unobservable subspace of the original system. On this basis, we establish the necessary and sufficient conditions for achieving observability matching. These theoretical findings motivate us to introduce a linear time-varying transformation to achieve a transformed system possessing a state-independent unobservable subspace. We prove the existence of such transformations and propose two design methodologies for constructing them. Moreover, we propose two equivalent consistent transformation-based EKF estimators, referred to as T-EKF 1 and T-EKF 2, respectively. T-EKF 1 employs the transformed system for consistent estimation, whereas T-EKF 2 leverages the original system but ensures consistency through state and covariance corrections from transformations. To validate our proposed methods, we conduct experiments on several representative examples, including multi-robot cooperative localization, multi-source target tracking, and 3D visual-inertial odometry, demonstrating that our approach achieves state-of-the-art performance in terms of accuracy, consistency, computational efficiency, and practical realizations.

Inconsistency issue is one crucial challenge for the performance of extended Kalman filter (EKF) based methods for state estimation problems, which is mainly affected by the discrepancy of observability between the EKF model and the underlying dynamic system. In this work, some sufficient and necessary conditions for observability maintenance are first proved. We find that under certain conditions, an EKF can naturally maintain correct observability if the corresponding linearization makes unobservable subspace independent of the state values. Based on this theoretical finding, a novel affine EKF (Aff-EKF) framework is proposed to overcome the inconsistency of standard EKF (Std-EKF) by affine transformations, which not only naturally satisfies the observability constraint but also has a clear design procedure. The advantages of our Aff-EKF framework over some commonly used methods are demonstrated through mathematical analyses. The effectiveness of our proposed method is demonstrated on three simultaneous localization and mapping (SLAM) applications with different types of features, typical point features, point features on a horizontal plane and plane features. Specifically, following the proposed procedure, the naturally consistent Aff-EKFs can be explicitly derived for these problems. The consistency improvement of these Aff-EKFs are validated by Monte Carlo simulations.

A method to construct an observability-constrained magnetic-field-aided inertial navigation system is proposed. The proposed method builds upon the previously proposed observability-constrained extended Kalman filter and extends it to work with a magnetic-field-based odometry-aided inertial navigation system. The proposed method is evaluated using simulation and real-world data, showing that (i) the system observability properties are preserved, (ii) the estimation accuracy increases, and (iii) the perceived uncertainty calculated by the EKF is more consistent with the true uncertainty of the filter estimates.

In this paper, we revisit the inconsistency problem of EKF-based cooperative localization (CL) from the perspective of system decomposition. By transforming the linearized system used by the standard EKF into its Kalman observable canonical form, the observable and unobservable components of the system are separated. Consequently, the factors causing the dimension reduction of the unobservable subspace are explicitly isolated in the state propagation and measurement Jacobians of the Kalman observable canonical form. Motivated by these insights, we propose a new CL algorithm called KD-EKF which aims to enhance consistency. The key idea behind the KD-EKF algorithm involves perform state estimation in the transformed coordinates so as to eliminate the influencing factors of observability in the Kalman observable canonical form. As a result, the KD-EKF algorithm ensures correct observability properties and consistency. We extensively verify the effectiveness of the KD-EKF algorithm through both Monte Carlo simulations and real-world experiments. The results demonstrate that the KD-EKF outperforms state-of-the-art algorithms in terms of accuracy and consistency.

Here we revisit the classic problem of linear quadratic estimation, i.e. estimating the trajectory of a linear dynamical system from noisy measurements. The celebrated Kalman filter gives an optimal estimator when the measurement noise is Gaussian, but is widely known to break down when one deviates from this assumption, e.g. when the noise is heavy-tailed. Many ad hoc heuristics have been employed in practice for dealing with outliers. In a pioneering work, Schick and Mitter gave provable guarantees when the measurement noise is a known infinitesimal perturbation of a Gaussian and raised the important question of whether one can get similar guarantees for large and unknown perturbations. In this work we give a truly robust filter: we give the first strong provable guarantees for linear quadratic estimation when even a constant fraction of measurements have been adversarially corrupted. This framework can model heavy-tailed and even non-stationary noise processes. Our algorithm robustifies the Kalman filter in the sense that it competes with the optimal algorithm that knows the locations of the corruptions. Our work is in a challenging Bayesian setting where the number of measurements scales with the complexity of what we need to estimate. Moreover, in linear dynamical systems past information decays over time. We develop a suite of new techniques to robustly extract information across different time steps and over varying time scales.